#include "csbmath.h"
#include "simple_collision.h"

#ifdef __cplusplus
extern "C" {
#endif

#define PP_SQUARE(x1, y1, x2, y2)	CSB_pow(y2-y1, 2) + CSB_pow(x2-x1, 2)

static bool p_in_c(float x, float y, const float* pc)
{
	return PP_SQUARE(x, y, pc[0], pc[1]) <= pc[2] * pc[2];
}

/* 使用点线斜率差判定是否在多边形内 */
static bool p_in_p(float x, float y, const float* pp, uint16_t n)
{
	int tmp;
	uint16_t c1 = 0, c2 = 0;
	uint16_t ix , iy, jx, jy;
	for (ix = 0, jx = n - 2; ix < n; ix += 2) {
		iy = ix + 1;
		jy = jx + 1;
		tmp = (int)((x - pp[jx]) * (pp[iy] - pp[jy]) -
				(y - pp[jy]) * (pp[ix] - pp[jx]));
		if (tmp > 0)
			++c1;
		else if (tmp < 0)
			++c2;
		jx = ix;
	}
	return !c1 || !c2;
}

static bool c_touch_c(const float* pc1, const float* pc2)
{
	return PP_SQUARE(pc1[0], pc1[1], pc2[0], pc2[1]) <= CSB_pow(pc1[2] + pc2[2], 2);
}

static bool c_touch_p(const float* pc, const float* pp, uint16_t n)
{
	// 通过求多边形每条边上法线上的两图形投影来判定碰撞
	bool testX = false;
	bool testY = false;
	float arr[SC_MAX_POL_NUM];
	float rate =  0;
	uint16_t k;
	uint16_t arrl = 0;
	float tmp1, tmp2;
	uint16_t ix , iy, jx, jy;
	float cmin, cmax, pmin, pmax;

	for (ix = 0, jx = n - 2; ix < n; ix += 2) {
		iy = ix + 1;
		jy = jx + 1;
		// i, j将组成一条边, 这里进行计算
		if (pp[ix] == pp[jx]) {
			if (testX)
				continue;
			testX = true;
			// 在y = 0上投影
			// 求多边形的
			pmin = pmax = pp[0];
			for (k = 2; k < n; k += 2) {
				if (pp[k] > pmax)
					pmax = pp[k];
				if (pp[k] < pmin)
					pmin = pp[k];
			}
			// 求圆的
			cmin = pc[0] - pc[2];
			cmax = pc[0] + pc[2];
		} else if (pp[iy] == pp[jy]) {
			if (testY)
				continue;
			testY = true;
			// 在x = 0上的投影
			// 求多边形的
			pmin = pmax = pp[1];
			for (k = 3; k < n; k += 2) {
				if (pp[k] > pmax)
					pmax = pp[k];
				if (pp[k] < pmin)
					pmin = pp[k];
			}
			// 求圆的
			cmin = pc[1] - pc[2];
			cmax = pc[1] + pc[2];
		} else {
			rate =  (pp[jx] - pp[ix]) / (pp[iy] - pp[jy]);
			for (k = 0; k < arrl; ++k) {
				if (arr[k] == rate)
					break;
			}
			if (k < arrl)
				continue;
			arr[arrl] = rate;
			++arrl;
			// 点坐标(n, m)在y = ax上的投影x = (n + a*m) / (a * a + 1)
			tmp1 = rate * rate + 1;
			// 求多边形的投影
			pmin = pmax = pp[0];
			for (k = 2; k < n; k += 2) {
				tmp2 = pp[k] + rate * pp[k + 1];
				if (tmp2 > pmax)
					pmax = tmp2;
				if (tmp2 < pmin)
					pmin = tmp2;
			}
			pmin = pmin / tmp1;
			pmax = pmax / tmp1;
			// 求圆的投影
			tmp2 = (pc[0] + rate * pc[1]) / tmp1;
			cmin = tmp2 - pc[2];
			cmax = tmp2 + pc[2];
		}
		// 执行判定
		if (cmax < pmin || cmin > pmax)
			return false;
		jx = ix;
	}
	return true;
}

static bool p_touch_p(const float* pp1, const float* pp2, uint16_t n1, uint16_t n2)
{
	uint16_t i;
	for (i = 0; i < n1; i += 2) {
		if (p_in_p(pp1[i], pp1[i + 1], pp2, n2))
			return true;
	}
	for (i = 0; i < n2; i += 2) {
		if (p_in_p(pp2[i], pp2[i + 1], pp1, n1))
			return true;
	}
	return false;
}

/*********************| 外部接口 |*********************/

/* 向SC结构中添加一个图元 */
CSB_DLL bool SC_add_ele(simple_collision_t* pSC, const float* points, int type, uint16_t n)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_RET(pSC && points && n >= SC_MIN_PNUM, CSBERR_PARAM, false);	
	n = (type == SC_ELE_CIRCLE) ? SC_CNUM : (n + n);
	CSB_YES_ERR_RET((SC_PNUM_LIMIT - pSC->pn) < n, CSBERR_BUF_NOT_ENOUGH, false);
	// 调整id和idx
	if (type == SC_ELE_CIRCLE) {
		pSC->idx[pSC->elen] = pSC->pn;
	} else {
		pSC->id[pSC->elen >> 5] |= 1 << (pSC->elen % 32);
		pSC->idx[pSC->elen] = n;
	}
	memcpy(pSC->points + pSC->pn, points, sizeof(float) * n);
	pSC->pn += n;
	++pSC->elen;
	return true;
}

CSB_DLL void SC_foreach(const simple_collision_t* pSC, sc_for_func func, void* udata)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_VRET(pSC && func, CSBERR_PARAM);
	uint16_t i = 0;
	uint16_t c = 0;
	while(i < pSC->elen) {
		if (SC_ELE_CIRCLE == (pSC->id[i >> 5] & (1 << (i % 32)))) {
			if (!func(SC_ELE_CIRCLE, &pSC->points[pSC->idx[i]], SC_CNUM, udata))
				return;
			c += SC_CNUM;
		} else {
			if (!func(SC_ELE_POLYGON, &pSC->points[c], pSC->idx[i], udata))
				return;
			c += pSC->idx[i];
		}
		++i;
	}

}

bool SC_hug_point(const pos_t* ppos, const simple_collision_t* pSC)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_RET(ppos && pSC && pSC->elen, CSBERR_PARAM, false);
	// 循环判定
	uint16_t i = 0;
	uint16_t c = 0;
	bool ret = false;
	while(i < pSC->elen) {
		if (SC_ELE_CIRCLE == (pSC->id[i >> 5] & (1 << (i % 32)))) {
			ret = p_in_c(ppos->x, ppos->y, &pSC->points[pSC->idx[i]]);
			c += SC_CNUM;
		} else {
			ret = p_in_p(ppos->x, ppos->y, &pSC->points[c], pSC->idx[i]);
			c += pSC->idx[i];
		}
		if (ret) return true;
		++i;
	}
	return false;
}

bool SC_love_SC(const simple_collision_t* pSC1, const simple_collision_t* pSC2)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_RET(pSC1 && pSC2 && pSC1->elen && pSC2->elen, CSBERR_PARAM, false);
	// 循环判定
	uint16_t i = 0;
	uint16_t c = 0;
	uint16_t j, k;
	bool ret = false;
	while(i < pSC1->elen) {
		if (SC_ELE_CIRCLE == (pSC1->id[i >> 5] & (1 << (i % 32)))) {
			// circle touch SC
			j = k = 0;
			while(j < pSC2->elen) {
				if (SC_ELE_CIRCLE == (pSC2->id[j >> 5] & (1 << (j % 32)))) {
					ret = c_touch_c(&pSC1->points[pSC1->idx[i]], &pSC2->points[pSC2->idx[j]]);
					k += SC_CNUM;
				} else {
					ret = c_touch_p(&pSC1->points[pSC1->idx[i]], &pSC2->points[k], pSC2->idx[j]);
					k += pSC2->idx[j];
				}
				if (ret) return true;
				++j;
			}
			c += SC_CNUM;
		} else {
			// polygon touch SC
			j = k = 0;
			while(j < pSC2->elen) {
				if (SC_ELE_CIRCLE == (pSC2->id[j >> 5] & (1 << (j % 32)))) {
					ret = c_touch_p(&pSC2->points[pSC2->idx[j]], &pSC1->points[c], pSC1->idx[i]);
					k += SC_CNUM;
				} else {
					ret = p_touch_p(&pSC1->points[c], &pSC2->points[k], pSC1->idx[i], pSC2->idx[j]);
					k += pSC2->idx[j];
				}
				if (ret) return true;
				++j;
			}
			c += pSC1->idx[i];
		}
		++i;
	}
	return false;
}

void SC_elebase_coll(const elebase_t* pele, const simple_collision_t* oriSC, simple_collision_t* out)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_VRET(pele && oriSC && out, CSBERR_PARAM);
	memcpy(out, oriSC, sizeof(simple_collision_t));
	// scale, rotate, move
	float tmpx=0, tmpy = 0;
	uint16_t i = 0;
	uint16_t c = 0;
	uint16_t tmp = 0;
	uint16_t j;
	while(i < out->elen) {
		tmp = out->idx[i];
		if (SC_ELE_CIRCLE == (out->id[i >> 5] & (1 << (i % 32)))) {
			// 对圆形进行计算。圆心进行普通运算，半径取x,y平均值缩放
			out->points[tmp + 2] *= (pele->scale.xs + pele->scale.ys) / 2;

			tmpx = out->points[tmp] * pele->scale.xs;
			tmpy = out->points[tmp + 1] * pele->scale.ys;
			CSB_rotate(&tmpx, &tmpy, pele->rotate);
			out->points[tmp] = tmpx + pele->pos.x;
			out->points[tmp + 1] = -tmpy + pele->pos.y;
			c += SC_CNUM;
		} else {
			for (j = c; j < tmp + c; j+=2) {
				tmpx = out->points[j] * pele->scale.xs;
				tmpy = out->points[j + 1] * pele->scale.ys;
				CSB_rotate(&tmpx, &tmpy, pele->rotate);
				out->points[j] = tmpx + pele->pos.x;
				out->points[j + 1] = -tmpy + pele->pos.y;
			}
			c += tmp;
		}
		++i;
	}
}

#ifdef __cplusplus
} /* end extern C */
#endif
